// generated with Atom
#include <WProgram.h>
#include "Compass.h"
#define COMPASS compass.heading()
#define STEERING steeringServo.write
#define THROTTLE throttleServo.write
#define assert //
Compass compass;

void loop(void);
int targetHeading = 1800;
int actualHeading = 0;
int steering = 0;
int bearing = 0;


static uint32_t __global_clock = 0;



static uint8_t __v7 = 0;  /* main.steering control left.right.s */
static uint8_t __v6 = 0;  /* main.steering control left.left.s */
static uint8_t __v5 = 0;  /* main.throtle */
static int16_t __v4 = 0;  /* main.rightSum */
static int16_t __v3 = 0;  /* main.leftSum */
static int16_t __v2 = 0;  /* main.dodgeSteering */
static uint8_t __v1 = 0;  /* main.moving */
static uint8_t __v0 = 0;  /* main.dodging */

static int16_t __a1[5] = {0,0,0,0,0};  /* main.rightDistanceMeasurements */
static int16_t __a0[5] = {0,0,0,0,0};  /* main.leftDistanceMeasurements */

/* main.read compass */
static void __r0(void) {
  uint8_t __0 = 1;
  int16_t __1 = COMPASS;
  if (__0) {
  }
  actualHeading = __1;
}

/* main.steering control left */
static void __r1(void) {
  uint8_t __0 = 1;
  uint8_t __1 = __v6 /* main.steering control left.left.s */ ;
  uint8_t __2 = __v7 /* main.steering control left.right.s */ ;
  uint8_t __3 = 75;
  uint8_t __4 = 90;
  uint8_t __5 = __2 ? __3 : __4;
  uint8_t __6 = 105;
  uint8_t __7 = __1 ? __5 : __6;
  int16_t __8 = 1800;
  int16_t __9 = actualHeading;
  int16_t __10 = targetHeading;
  int16_t __11 = __9 - __10;
  uint8_t __12 = __8 < __11;
  int16_t __13 = 3600;
  int16_t __14 = __10 + __13;
  int16_t __15 = __12 ? __14 : __10;
  int16_t __16 = __10 - __9;
  uint8_t __17 = __8 < __16;
  int16_t __18 = __9 + __13;
  int16_t __19 = __17 ? __18 : __9;
  int16_t __20 = __15 - __19;
  if (__0) {
    printf("target': %i\n", __15);
    printf("actual': %i\n", __19);
    printf("difference: %i\n", __20);
    printf("steering: %i\n", __7);
    printf("-------------------------------------------\n");
  }
  steering = __7;
}

/* main.steering control left.left */
static void __r2(void) {
  uint8_t __0 = 1;
  uint8_t __1 = __v6 /* main.steering control left.left.s */ ;
  int16_t __2 = 1800;
  int16_t __3 = targetHeading;
  int16_t __4 = actualHeading;
  int16_t __5 = __3 - __4;
  uint8_t __6 = __2 < __5;
  int16_t __7 = 3600;
  int16_t __8 = __4 + __7;
  int16_t __9 = __6 ? __8 : __4;
  int16_t __10 = __4 - __3;
  uint8_t __11 = __2 < __10;
  int16_t __12 = __3 + __7;
  int16_t __13 = __11 ? __12 : __3;
  int16_t __14 = 100;
  int16_t __15 = __13 - __14;
  uint8_t __16 = __13 < __15;
  int16_t __17 = __16 ? __13 : __15;
  uint8_t __18 = __9 < __17;
  uint8_t __19 = ! __18;
  uint8_t __20 = __15 < __13;
  int16_t __21 = __20 ? __13 : __15;
  uint8_t __22 = __21 < __9;
  uint8_t __23 = ! __22;
  uint8_t __24 = __1 && __19 && __23;
  uint8_t __25 = ! __24;
  uint8_t __26 = __25 && __23;
  uint8_t __27 = ! __26;
  if (__0) {
  }
  __v6 /* main.steering control left.left.s */ = __27;
}

/* main.steering control left.right */
static void __r3(void) {
  uint8_t __0 = 1;
  uint8_t __1 = __v7 /* main.steering control left.right.s */ ;
  int16_t __2 = 1800;
  int16_t __3 = targetHeading;
  int16_t __4 = actualHeading;
  int16_t __5 = __3 - __4;
  uint8_t __6 = __2 < __5;
  int16_t __7 = 3600;
  int16_t __8 = __4 + __7;
  int16_t __9 = __6 ? __8 : __4;
  int16_t __10 = __4 - __3;
  uint8_t __11 = __2 < __10;
  int16_t __12 = __3 + __7;
  int16_t __13 = __11 ? __12 : __3;
  int16_t __14 = 100;
  int16_t __15 = __13 + __14;
  uint8_t __16 = __15 < __13;
  int16_t __17 = __16 ? __15 : __13;
  uint8_t __18 = __9 < __17;
  uint8_t __19 = ! __18;
  uint8_t __20 = __13 < __15;
  int16_t __21 = __20 ? __15 : __13;
  uint8_t __22 = __21 < __9;
  uint8_t __23 = ! __22;
  uint8_t __24 = __1 && __19 && __23;
  uint8_t __25 = ! __24;
  uint8_t __26 = __25 && __23;
  uint8_t __27 = ! __26;
  if (__0) {
  }
  __v7 /* main.steering control left.right.s */ = __27;
}

/* main.distance measurements */
static void __r6(void) {
  uint8_t __0 = __v0 /* main.dodging */ ;
  int16_t __1 = newestRight;
  int16_t __2 = 0;
  int16_t __3 = __a1[__2] /* main.rightDistanceMeasurements */ ;
  int16_t __4 = __1 + __3;
  int16_t __5 = 1;
  int16_t __6 = __a1[__5] /* main.rightDistanceMeasurements */ ;
  int16_t __7 = __4 + __6;
  int16_t __8 = 2;
  int16_t __9 = __a1[__8] /* main.rightDistanceMeasurements */ ;
  int16_t __10 = __7 + __9;
  int16_t __11 = 3;
  int16_t __12 = __a1[__11] /* main.rightDistanceMeasurements */ ;
  int16_t __13 = __10 + __12;
  int16_t __14 = 4;
  int16_t __15 = __a1[__14] /* main.rightDistanceMeasurements */ ;
  int16_t __16 = __13 + __15;
  int16_t __17 = __v4 /* main.rightSum */ ;
  int16_t __18 = __0 ? __16 : __17;
  int16_t __19 = newestLeft;
  int16_t __20 = __a0[__2] /* main.leftDistanceMeasurements */ ;
  int16_t __21 = __19 + __20;
  int16_t __22 = __a0[__5] /* main.leftDistanceMeasurements */ ;
  int16_t __23 = __21 + __22;
  int16_t __24 = __a0[__8] /* main.leftDistanceMeasurements */ ;
  int16_t __25 = __23 + __24;
  int16_t __26 = __a0[__11] /* main.leftDistanceMeasurements */ ;
  int16_t __27 = __25 + __26;
  int16_t __28 = __a0[__14] /* main.leftDistanceMeasurements */ ;
  int16_t __29 = __27 + __28;
  int16_t __30 = __v3 /* main.leftSum */ ;
  int16_t __31 = __0 ? __29 : __30;
  int16_t __32 = __0 ? __12 : __15;
  int16_t __33 = __0 ? __9 : __12;
  int16_t __34 = __0 ? __6 : __9;
  int16_t __35 = __0 ? __3 : __6;
  int16_t __36 = __0 ? __1 : __3;
  int16_t __37 = __0 ? __26 : __28;
  int16_t __38 = __0 ? __24 : __26;
  int16_t __39 = __0 ? __22 : __24;
  int16_t __40 = __0 ? __20 : __22;
  int16_t __41 = __0 ? __19 : __20;
  if (__0) {
  }
  __v4 /* main.rightSum */ = __18;
  __v3 /* main.leftSum */ = __31;
  __a1[__14] /* main.rightDistanceMeasurements */ = __32;
  __a1[__11] /* main.rightDistanceMeasurements */ = __33;
  __a1[__8] /* main.rightDistanceMeasurements */ = __34;
  __a1[__5] /* main.rightDistanceMeasurements */ = __35;
  __a1[__2] /* main.rightDistanceMeasurements */ = __36;
  __a0[__14] /* main.leftDistanceMeasurements */ = __37;
  __a0[__11] /* main.leftDistanceMeasurements */ = __38;
  __a0[__8] /* main.leftDistanceMeasurements */ = __39;
  __a0[__5] /* main.leftDistanceMeasurements */ = __40;
  __a0[__2] /* main.leftDistanceMeasurements */ = __41;
}

/* main.drive */
static void __r4(void) {
  uint8_t __0 = 1;
  uint8_t __1 = steering;
  uint8_t __2 = __v5 /* main.throtle */ ;
  if (__0) {
    STEERING(__1); THROTTLE(__2);
  }
}

/* main.calculate dodge */
static void __r5(void) {
  uint8_t __0 = __v0 /* main.dodging */ ;
  int16_t __1 = 1200;
  int16_t __2 = __v3 /* main.leftSum */ ;
  uint8_t __3 = __1 < __2;
  uint8_t __4 = ! __3;
  int16_t __5 = __v4 /* main.rightSum */ ;
  uint8_t __6 = __1 < __5;
  uint8_t __7 = ! __6;
  uint8_t __8 = __4 && __7;
  uint8_t __9 = ! __8;
  uint8_t __10 = __0 && __9;
  int16_t __11 = __2 - __5;
  int16_t __12 = __v2 /* main.dodgeSteering */ ;
  int16_t __13 = __10 ? __11 : __12;
  if (__10) {
  }
  __v2 /* main.dodgeSteering */ = __13;
}


static void __assertion_checks(void) {
  uint8_t __0 = 1;
  int16_t __1 = 1800;
  int16_t __2 = targetHeading;
  int16_t __3 = actualHeading;
  int16_t __4 = __2 - __3;
  uint8_t __5 = __1 < __4;
  int16_t __6 = 3600;
  int16_t __7 = __3 + __6;
  int16_t __8 = __5 ? __7 : __3;
  int16_t __9 = __3 - __2;
  uint8_t __10 = __1 < __9;
  int16_t __11 = __2 + __6;
  int16_t __12 = __10 ? __11 : __2;
  int16_t __13 = __8 - __12;
  uint8_t __14 = __1 < __13;
  uint8_t __15 = ! __14;
  int16_t __16 = __12 - __8;
  uint8_t __17 = __1 < __16;
  uint8_t __18 = ! __17;
  if (__0) assert(0, __15, __global_clock);
  if (__0) assert(1, __18, __global_clock);
}


void loop(void) {
  {
    static uint8_t __scheduling_clock = 0;
    if (__scheduling_clock == 0) {
      __assertion_checks(); __r0();  /* main.read compass */
      __assertion_checks(); __r1();  /* main.steering control left */
      __assertion_checks(); __r2();  /* main.steering control left.left */
      __assertion_checks(); __r3();  /* main.steering control left.right */
      __assertion_checks(); __r6();  /* main.distance measurements */
      __scheduling_clock = 0;
    }
    else {
      __scheduling_clock = __scheduling_clock - 1;
    }
  }
  {
    static uint8_t __scheduling_clock = 0;
    if (__scheduling_clock == 0) {
      __assertion_checks(); __r4();  /* main.drive */
      __scheduling_clock = 4;
    }
    else {
      __scheduling_clock = __scheduling_clock - 1;
    }
  }
  {
    static uint8_t __scheduling_clock = 0;
    if (__scheduling_clock == 0) {
      __assertion_checks(); __r5();  /* main.calculate dodge */
      __scheduling_clock = 1;
    }
    else {
      __scheduling_clock = __scheduling_clock - 1;
    }
  }

  __global_clock = __global_clock + 1;
}

void main() {
  init();
  compass.initialize();
  for (;;) {
    loop();
  }
}

